#include "gps_module.h"

#include <fstream>
#include <iostream>
#include <stdlib.h>
#include <string>
#include <unistd.h>
using namespace std;
namespace sensor::gps {

static muduo::net::EventLoopThread GpsReceiverThread;

bool GPSModule::serial_data_analyse()
{
    bool ret = true;
    int len = 0;
    char once_data[REC_BUFF_LEN];
    char data_back[REC_BUFF_LEN];
    int len_back = 0;
    memset(once_data, 0, REC_BUFF_LEN * sizeof(char));
    len = _serial_port_ptr->read(once_data, REC_BUFF_LEN);
    cout << "Debug: Data length for one read: " << len << endl;
    if (len <= 0) {
        // cout << "[read]: 本次串口没有数据-total_len: " << total_len << endl;
        if (_total_len < 1) {
            return false;
        } else if (_total_len >= REC_BUFF_LEN) {
            cout << "[Debug][read]: Data too long, take it as abnormal data" << endl;
            _total_len = 0;
            memset(_buff, 0, REC_BUFF_LEN * sizeof(char));
            return false;
        } else {
            memset(once_data, 0, REC_BUFF_LEN * sizeof(char));
            memcpy(once_data, _buff, _total_len);

            memset(_buff, 0, REC_BUFF_LEN * sizeof(char));
            memset(_pvt_data, 0, BUFFER_128 * sizeof(char)); // 100字节内容
            memset(_imu_data, 0, BUFFER_128 * sizeof(char)); // 44字节内容
            memset(_rmc_data, 0, BUFFER_128 * sizeof(char)); // 75字节内容左右，长度不固定
            cout << "Full GPS Data: " << len << endl;
            cout << "####################################################################" << endl;
            for (int i = 0; i < _total_len; i++) {
                printf("%02X-", (unsigned char)once_data[i]);
                if ((i + 1) % 20 == 0) {
                    cout << endl;
                }
            }
            cout << "####################################################################" << endl;
        }
    } else {
        cout << "读取的数据长度: " << len;
        cout << "********************************************************************" << endl;
        for (int i = 0; i < len; i++) {
            printf("%02X-", (unsigned char)once_data[i]);
            if ((i + 1) % 20 == 0) {
                cout << endl;
            }
        }
        cout << "********************************************************************" << endl;
        if (((unsigned char)once_data[0] == 0xB5) && ((unsigned char)once_data[1] == 0x62) && ((unsigned char)once_data[2] == 0x01) && ((unsigned char)once_data[3] == 0x07)) {
            if ((!_total_len) || (_total_len >= REC_BUFF_LEN)) {
                memset(_buff, 0, REC_BUFF_LEN * sizeof(char));
                memcpy(_buff, once_data, len);
                // DEBUG_ERR("[read]: 数据长度过长, total_len-%d\n", total_len);
                _total_len = len;
                return false;
            }

            memset(data_back, 0, REC_BUFF_LEN * sizeof(char));
            memcpy(data_back, once_data, len);
            len_back = len;

            memset(once_data, 0, REC_BUFF_LEN * sizeof(char));
            memcpy(once_data, _buff, _total_len);

            memset(_buff, 0, REC_BUFF_LEN * sizeof(char));
            memcpy(_buff, data_back, len);
        } else {
            int pos = 0;
            while (pos < (len - 3)) {
                if (((unsigned char)once_data[pos] == 0xB5) && ((unsigned char)once_data[pos + 1] == 0x62) && ((unsigned char)once_data[pos + 2] == 0x01) && ((unsigned char)once_data[pos + 3] == 0x07)) {
                    memset(data_back, 0, REC_BUFF_LEN * sizeof(char));
                    memcpy(data_back, once_data + pos, len - pos);
                    len_back = (len - pos);

                    if (_total_len >= (REC_BUFF_LEN - pos)) {
                        _total_len = len_back;
                        memset(_buff, 0, REC_BUFF_LEN * sizeof(char));
                        memcpy(_buff, data_back, len_back);
                        return false;
                    } else {
                        memcpy(_buff + _total_len, once_data, pos);
                        _total_len += pos;

                        memset(once_data, 0, REC_BUFF_LEN * sizeof(char));
                        memcpy(once_data, _buff, _total_len);

                        memset(_buff, 0, REC_BUFF_LEN * sizeof(char));
                        memcpy(_buff, data_back, len_back);
                    }

                    ret &= _decoder.Get_PVT_Data(once_data, _pvt_data, _total_len);
                    ret &= _decoder.Get_IMU_Data(once_data, _imu_data, _total_len);
                    ret &= _decoder.Get_RMC_Data(once_data, _rmc_data, _total_len);
                    _total_len = 0;
                    if (len_back > 0) {
                        _total_len = len_back;
                    }
                    if (false == ret) {
                        cout << "[serial_data_analyse]: 数据不全" << endl;
                        memset(_pvt_data, 0, BUFFER_128 * sizeof(char));
                        memset(_imu_data, 0, BUFFER_128 * sizeof(char));
                        memset(_rmc_data, 0, BUFFER_128 * sizeof(char));
                        return false;
                    }
                    return true;
                }
                pos++;
            }

            if (_total_len >= (REC_BUFF_LEN - len)) {
                std::cout << "[read]: 现有读取数据已经较长" << endl;
                _total_len = 0;
                memset(_buff, 0, REC_BUFF_LEN * sizeof(char));
                return false;
            } else if (_total_len) {
                memcpy(_buff + _total_len, once_data, len);
                _total_len += len;
                return false;
            } else {
                return false;
            }
        }
    }

    ret &= _decoder.Get_PVT_Data(once_data, _pvt_data, _total_len);
    ret &= _decoder.Get_IMU_Data(once_data, _imu_data, _total_len);
    ret &= _decoder.Get_RMC_Data(once_data, _rmc_data, _total_len);
    _total_len = 0;
    if (len_back > 0) {
        _total_len = len_back;
    }
    if (false == ret) {
        cout << "[serial_data_analyse]: 数据不全" << endl;

        memset(_pvt_data, 0, BUFFER_128 * sizeof(char));
        memset(_imu_data, 0, BUFFER_128 * sizeof(char));
        memset(_rmc_data, 0, BUFFER_128 * sizeof(char));
        return false;
    }
    return true;
}

void GPSModule::setCallback(const onGpsCallback& cb)
{
    cb_ = cb;
}

void GPSModule::thread_read_serial_data()
{
    while (true) {
        usleep(GPSREADINTERVAL);
        // if (_sensor_global._gps_infos[get_device_id()].init_status != int(InitStatus::SUCCESS)) {
        //     /*TODO:Try to reinitialize GPS module*/
        //     continue;
        // }
        bool ret = serial_data_analyse();
        if (ret) {
            IMUInfo imu_msg = { 0 };
            GPSData gps_data_cur = { 0 };

            /*Decode message*/
            _decoder.decode_pvt(_pvt_data, gps_data_cur);
            _decoder.decode_imu(_imu_data, imu_msg, gps_data_cur);
            _decoder.decode_rmc(_rmc_data, gps_data_cur);
            std::cout << "Received gps data: latitude->" << gps_data_cur.lla.latitude << " longitude->"
                      << gps_data_cur.lla.longitude << std::endl;
            auto now = muduo::Timestamp::now();
            interface::msg::Gnss gnnsData;
            gnnsData.header.stamp.sec = now.secondsSinceEpoch();
            gnnsData.header.stamp.nanosec = (now.microSecondsSinceEpoch() % 1000000) * 1000;
            gnnsData.set__latitude(gps_data_cur.lla.latitude);
            gnnsData.set__longitude(gps_data_cur.lla.longitude);
            gnnsData.set__elevation(gps_data_cur.lla.height_over_sea);
            gnnsData.set__heading(gps_data_cur.speed_cog.cog);
            gnnsData.set__speed(gps_data_cur.speed_cog.speed);
            /* Set position info */

            if ((gps_data_cur.lla.latitude > 1) && (gps_data_cur.lla.longitude > 1)) {
                gnnsData.set__valid(true);
            } else {
                gnnsData.set__valid(false);
                std::cout << "Received gps data latitude&longitude not greater than 1,take it "
                             "as wrong data. May because it was taken indoors."
                          << std::endl;
                // _sensor_global.gps_infos[get_device_id()].run_status = int(RunStatus::FAILURE);
            }
            cb_(gnnsData);
            std::cout << "Sending one GPS message." << std::endl;
            ++_index;
        }
    }
}

// TODO: add error reconnect process
GPSModule::GPSModule(std::string device_address, muduo::net::EventLoop* loop)
{
    _buff = new char[REC_BUFF_LEN];
    memset(_buff, 0, REC_BUFF_LEN * sizeof(char));
    _total_len = 0;

    OpenOptions gps_serial_port_option;
    gps_serial_port_option.auto_open = false;
    gps_serial_port_option.baud_rate = BR115200;
    gps_serial_port_option.data_bits = DATA_BITS_8;
    gps_serial_port_option.parity = PARITY_NONE;
    gps_serial_port_option.stop_bits = STOP_BITS_1;
    gps_serial_port_option.vmin = 0;
    gps_serial_port_option.vtime = 50;
    gps_serial_port_option.xany = false;
    gps_serial_port_option.xoff = false;
    gps_serial_port_option.xon = false;

    _index = 1;

    _serial_port_ptr = std::make_shared<SerialPort>(device_address, gps_serial_port_option);
    bool ret = _serial_port_ptr->start();
    if (!ret) {
        std::cout << "串口初始化失败" << std::endl;
    }
    auto pool = read_frame_loop_.startLoop();
    pool->runInLoop([=] {
        thread_read_serial_data();
    });

    // end of ctor
}

GPSModule::~GPSModule()
{
    if (_buff) {
        delete[] _buff;
    }
}

} // namespace sensor::gps
